#include <avr/io.h>
#include <util/delay.h>
#include "uart/uart.h"
#include <stdint.h>
#include "joystick/joystick.h"
#include "motor/motor.h"
#include "common.h"
#include "servo/servo.h"
#include "radio/radio.h"
#include "sonar.h" //sonar now in common folder


int main(void) {
	Disable_Interrupt();
	CLKPR = 0x80;
	CLKPR = 0x00;

	sonar_init();
	
	DDRD = _BV(PORTD4);
	uart_init();
	joystick_init();
	//calibrate_joystick(0, 0);
	pwm_motors_init();
	init_servo();
	Enable_Interrupt();

	_delay_ms(20); 

	//char message[13] = "A Message\r\n";
	unsigned char joystick_x;
	unsigned char joystick_y;
	unsigned char joystick_t;
	
	uint8_t distance = 0;
	
	//uart_write((uint8_t*)&message);

	//char buf[10];

	
	
	for(;;) {

	/*	sample_joystick(&joystick_x, &joystick_y, &joystick_t);
		_delay_ms(100);
	
		uart_write("X: ");
		uart_write(itoa(joystick_x, buf,10));

		uart_write("    Y: ");
		uart_write(itoa(joystick_y, buf,10));
	/*
		uart_write("    T: ");
		uart_write(itoa(joystick_t, buf,10));
		uart_write("\r\n");

	*/
	
		
	

		//PORTD ^= _BV(PORTD4);
		//_delay_ms(200);

		//pwm_set_duty (RIGHT_MOTOR, joystick_y_transform(joystick_y));
		
		//servo_setDutyCycleA(joystick_x_transform(joystick_x));

		//
		trigger_sonar(); 
		distance = read_distance();		

		uint8_t i = 0;
		uint8_t j = 0;		
		
		for (i = 0; i< distance; i++){
			PORTD ^= _BV(PORTD4);
			for (j =0 ; j<5; j++){
				_delay_ms(100);
			}	
		}
		
		//pause before repeating
		for(i=0; i<7; i++)
		{
			_delay_ms(100);		
		}

		//PORTD ^= _BV(PORTD4);
			
	}

}


int joystick_y_transform(int y) {

	int ref = 62;

	if( y > ref )
		return (y - ref) * 2;
	else
		return (ref - y) * -2;

}

int joystick_x_transform(int x) {

	return (10.75*x - 66.5);

}
